/*
https://github.com/sebnil/Moving-Avarage-Filter--Arduino-Library-
*/
#include "MovingAverage.h"

void MovingAverageFilter::init(unsigned int newDataPointsCount) {
  if (newDataPointsCount < MAX_DATA_POINTS)
	dataPointsCount = newDataPointsCount;
  else
	dataPointsCount = MAX_DATA_POINTS;
  sum = 0;
  k = 0;
  init_flag = 0;
}

int MovingAverageFilter::process(int in) {

  int i;
  if(init_flag == 0) {
    sum = in*dataPointsCount;
    for (i=0; i<dataPointsCount; i++) {
      values[i] =in; // fill the array with first sample
    }    
    k = 0;
    init_flag = 1;
    return in;
  }
  else {
    sum += in-values[k];
    values[k] = in;
    k++;
    if(k>=dataPointsCount) k = 0;
    return sum/dataPointsCount;
  }
}

int MovingAverageFilter::round_process(int in) {

  int i,plus_count,sum,v;
  if(init_flag == 0) {
    sum = in*dataPointsCount;
    for (i=0; i<dataPointsCount; i++) {
      values[i] =in; // fill the array with first sample
    }    
    k = 0;
    init_flag = 1;
    return in;
  }
  else {
    values[k] = in;
    k++;
    if(k>=dataPointsCount) k = 0;
    plus_count = 0;   
    for(i=0;i<dataPointsCount;i++){
      if(values[i]>=0) plus_count++;
    }
    sum = 0; 
    if(plus_count && (plus_count != dataPointsCount)){ // plus, minus mix    
      if((values[0]>9000) || (values[0]< -9000)) {// around 180 deg
        for(i=0;i<dataPointsCount;i++) {
          v = values[i];
          if(v<0) v += 36000;
          sum += v;
        }
        v = sum/dataPointsCount;
        if(v>18000) v = 36000-v;
        return v;        
      }
    }
    for(i=0;i<dataPointsCount;i++) sum += values[i];
    return sum/dataPointsCount;
  }
}


